#include "motor.h"

#define MAX_SPEED_UP 5
#define MAX_SPEED_CUT 1

volatile float MotorA_Speed;
volatile float MotorB_Speed;

// 设置电机的占空比
void Motor_Set(int Motor1, int Motor2) {
    // 先根据正负设置方向GPIO 高低点平
    if (Motor1 < 0) {
        AIN1_RESET();
        AIN2_SET();
    } else {
        AIN1_SET();
        AIN2_RESET();
    }
    if (Motor2 < 0) {
        BIN1_RESET();
        BIN2_SET();
    } else {
        BIN1_SET();
        BIN2_RESET();
    }

    if (Motor1 < 0) {
        if (Motor1 < -99) {
            Motor1 = -99;
        }
        SET_PWMA_COMPARE(0 - Motor1);
    } else {
        if (Motor1 > 99) {
            Motor1 = 99;
        }
        SET_PWMA_COMPARE(Motor1);
    }
    if (Motor2 < 0) {
        if (Motor2 < -99) {
            Motor2 = -99;
        }
        SET_PWMB_COMPARE(0 - Motor2);
    } else {
        if (Motor2 > 99) {
            Motor2 = 99;
        }
        SET_PWMB_COMPARE(Motor2);
    }
}


/*
* @brief	设置电机的速度
* @param	电机A的速度  右边
* @param	电机B的速度  左边
* @return	无
*/
void Motor_Set_Speed(float MotorA_Set_Speed, float MotorB_Set_Speed) {
    pidA.target_val = -MotorA_Set_Speed;
    pidB.target_val = -MotorB_Set_Speed;
    Motor_Set(PID_realize(&pidA, MotorA_Speed), PID_realize(&pidB, MotorB_Speed));
}

/*
* @brief    加速度
* @param
* @return
*/
void Motor_SpeedUP(void) {
    static float motor_speed_up = 0.5;
    if (motor_speed_up <= MAX_SPEED_UP) motor_speed_up += (float) 0.5;
    Motor_Set_Speed(motor_speed_up, motor_speed_up);
}

/*
* @brief    减速度
* @param
* @return
*/
void Motor_SpeedCut(void) {
    static float motor_speed_cut = 3.0;
    if (motor_speed_cut >= MAX_SPEED_CUT) motor_speed_cut -= (float) 0.5;
    Motor_Set_Speed(motor_speed_cut, motor_speed_cut);
}
